# -*- coding: utf-8 -*-
import os
import re
import json
import threading
import subprocess
from math import pi

import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import String
from nav_msgs.msg import Odometry
from sensor_msgs.msg import BatteryState
import paho.mqtt.client as mqtt

from ros_mqtt.robotsys import robotsys
from ros_mqtt.robotmap import robotmap
from ros_mqtt.cleanwork import cleanwork
from ros_mqtt.robotbox import robotbox

class CommandSender(object):
    def __init__(self, node_name):
        self.node_name = node_name
        self.host = "mq.richwin8.com" # mq.richwin8.com
        self.port = "1883"
        self.topic = "Robot.south.866562"
        self.mqtt_pub = rospy.Publisher("MQTTReceiver", String, queue_size=20)
        self.__client = mqtt.Client(protocol=mqtt.MQTTv311)
        self.__client.on_connect = self._on_connect
        self.__client.on_message = self._on_message
        self.robot_status = "Free"
        self.data = None
        self.robot_sys = robotsys()
        self.robot_map = robotmap()
        self.clean_work = cleanwork()
        self.robot_box = robotbox()
        rospy.on_shutdown(self.__client.disconnect)
        rospy.on_shutdown(self.__client.loop_stop)

    def connect(self):
        rospy.loginfo('Connect mqtt broker')
        self.__client.connect(self.host, port=self.port, keepalive=60)
        self.__client.loop_start()
        return self

    def start(self):
        rospy.loginfo('AttributeReceiver start :  %s'%self.node_name)
        rospy.spin()
        rospy.loginfo('AttributeReceiver stop : %s'%self.node_name)

    def _on_connect(self, client, userdata, flags, response_code):
        rospy.loginfo('mqtt connect status %s'%response_code)
        client.subscribe("Robot.south.866562")

    def _on_message(self, client, userdata, msg):
        payload = str(msg.payload)
        rospy.loginfo('mqtt_receiver command msg :  %s'%payload)
        msg_json = eval(msg.payload)
        cmd = msg_json['method']
        if cmd == 'system.status':
            self.data = self.robot_sys.status(self.robot_status)
        elif cmd == 'system.speed':
            self.robot_status = "Busy"
            self.data = self.robot_sys.speed()
        elif cmd == 'system.move':
            self.robot_status = "Busy"
            speed_x = msg_json['data']['speed_x']
            speed_y = msg_json['data']['speed_y']
            speed_angle = msg_json['data']['speed_angle']
            self.data = self.robot_sys.move(speed_x, speed_y, speed_angle)
        elif cmd == 'system.shutdown':
            self.robot_status = "Free"
            self.data = self.robot_sys.shutdown()
        elif cmd == 'system.pose':
            self.data = self.robot_sys.pose()
        elif cmd == 'map.start':
            self.robot_status = "Mapping"
            self.data = self.robot_map.start()
        elif cmd == 'map.stop':
            self.robot_status = "Free"
            self.data = self.robot_map.stop()
        elif cmd == 'map.upload':
            self.robot_status = "Free"
            self.data = self.robot_map.upload()
        elif cmd == 'map.download':
            url = msg_json['data']['url']
            self.data = self.robot_map.download(url)
        elif cmd == 'map.space_arrange':
            self.robot_status = "Free"
            home = msg_json['data']['home']
            count = msg_json['data']['count']
            tables = msg_json['data']['tables']
            self.data = self.robot_map.space_arrange(home, count, tables)
        elif cmd == 'cleanwork.return':
            self.robot_status = "Charging"
            self.data = self.clean_work.chargeback()
        elif cmd == 'cleanwork.start':
            self.robot_status = "Cleaning"
            # path = msg_json['data']['path']
            self.data = self.clean_work.start()
        elif cmd == 'cleanwork.report':
            self.robot_status = "Cleaning"
            self.data = self.clean_work.report()
        elif cmd == 'cleanwork.stop':
            self.robot_status = "Free"
            self.data = self.clean_work.stop()
        elif cmd == 'box.open':
            self.data = self.robot_box.open()
        elif cmd == 'box.close':
            self.data = self.robot_box.close()
        rospy.sleep(0.2)
        if self.data:
            self.mqtt_pub.publish(str(self.data))
        rospy.loginfo('active threads = %s' % threading.active_count())
